#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"
#include "turtlesim/srv/spawn.hpp"
#include "rclcpp/utilities.hpp"
#include <cmath>
#include <memory>

using namespace std::chrono_literals;

class TurtleChaseNode : public rclcpp::Node
{
public:
    TurtleChaseNode() : Node("turtle_chase_node")
    {
        // 初始化变量
        turtle1_pose_received_ = false;
        turtle2_pose_received_ = false;
        
        // 创建发布者 - 控制第二只乌龟
        turtle2_cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle2/cmd_vel", 10);
        
        // 创建订阅者 - 监听两只乌龟的位置
        turtle1_pose_sub_ = this->create_subscription<turtlesim::msg::Pose>(
            "/turtle1/pose", 10, 
            std::bind(&TurtleChaseNode::turtle1_pose_callback, this, std::placeholders::_1));
            
        turtle2_pose_sub_ = this->create_subscription<turtlesim::msg::Pose>(
            "/turtle2/pose", 10, 
            std::bind(&TurtleChaseNode::turtle2_pose_callback, this, std::placeholders::_1));
        
        // 创建客户端 - 用于生成第二只乌龟
        spawn_client_ = this->create_client<turtlesim::srv::Spawn>("/spawn");
        
        // 创建定时器 - 控制追逐逻辑
        control_timer_ = this->create_wall_timer(50ms, std::bind(&TurtleChaseNode::control_callback, this));
        
        // 等待spawn服务可用
        this->wait_for_spawn_service();
        
        // 生成第二只乌龟
        this->spawn_turtle2();
        
        RCLCPP_INFO(this->get_logger(), "乌龟追逐节点已启动");
    }

private:
    // 发布者和订阅者
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr turtle2_cmd_pub_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr turtle1_pose_sub_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr turtle2_pose_sub_;
    rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawn_client_;
    rclcpp::TimerBase::SharedPtr control_timer_;
    
    // 乌龟位置信息
    turtlesim::msg::Pose turtle1_pose_;
    turtlesim::msg::Pose turtle2_pose_;
    bool turtle1_pose_received_;
    bool turtle2_pose_received_;
    
    // 控制参数
    double max_linear_speed_ = 2.0;
    double max_angular_speed_ = 2.0;
    double chase_distance_ = 1.0;  // 保持的追逐距离
    
    void wait_for_spawn_service()
    {
        while (!spawn_client_->wait_for_service(1s)) {
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(), "等待spawn服务时被中断");
                return;
            }
            RCLCPP_INFO(this->get_logger(), "等待spawn服务...");
        }
    }
    
    void spawn_turtle2()
    {
        auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
        request->x = 5.0;  // 初始位置
        request->y = 5.0;
        request->theta = 0.0;
        request->name = "turtle2";
        
        auto future = spawn_client_->async_send_request(request);
        
        // 等待生成完成
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) ==
            rclcpp::FutureReturnCode::SUCCESS) {
            RCLCPP_INFO(this->get_logger(), "第二只乌龟(turtle2)生成成功");
        } else {
            RCLCPP_ERROR(this->get_logger(), "生成第二只乌龟失败");
        }
    }
    
    void turtle1_pose_callback(const turtlesim::msg::Pose::SharedPtr msg)
    {
        turtle1_pose_ = *msg;
        turtle1_pose_received_ = true;
    }
    
    void turtle2_pose_callback(const turtlesim::msg::Pose::SharedPtr msg)
    {
        turtle2_pose_ = *msg;
        turtle2_pose_received_ = true;
    }
    
    // 规范化角度到 [-π, π] 范围
    double normalize_angle(double angle)
    {
        while (angle > M_PI) angle -= 2 * M_PI;
        while (angle < -M_PI) angle += 2 * M_PI;
        return angle;
    }
    
    void control_callback()
    {
        if (!turtle1_pose_received_ || !turtle2_pose_received_) {
            return;  // 等待收到两只乌龟的位置信息
        }
        
        auto message = geometry_msgs::msg::Twist();
        
        // 计算两只乌龟之间的距离和角度
        double dx = turtle1_pose_.x - turtle2_pose_.x;
        double dy = turtle1_pose_.y - turtle2_pose_.y;
        
        double distance = std::sqrt(dx * dx + dy * dy);
        double target_angle = std::atan2(dy, dx);
        
        // 计算角度差
        double angle_diff = normalize_angle(target_angle - turtle2_pose_.theta);
        
        // 控制逻辑
        if (distance > chase_distance_) {
            // 追逐模式：朝向目标并前进
            double kp_angular = 4.0;
            double angular_control = kp_angular * angle_diff;
            
            // 限制角速度
            if (angular_control > max_angular_speed_) angular_control = max_angular_speed_;
            if (angular_control < -max_angular_speed_) angular_control = -max_angular_speed_;
            
            message.angular.z = angular_control;
            
            // 线速度控制：距离越远速度越快，但不超过最大值
            double desired_speed = 0.5 * (distance - chase_distance_);
            message.linear.x = std::min(desired_speed, max_linear_speed_);
            
            // 如果角度差太大，先转向再前进
            if (std::abs(angle_diff) > M_PI/4) {  // 如果角度差大于45度
                message.linear.x = 0.0;  // 先停下来转向
            }
        } else {
            // 已经接近目标，减速并保持距离
            message.linear.x = 0.0;
            message.angular.z = 0.0;
        }
        
        // 发布控制命令
        turtle2_cmd_pub_->publish(message);
        
        // 输出调试信息
        RCLCPP_DEBUG(this->get_logger(), 
                    "距离: %.2f, 角度差: %.2f°, 线速度: %.2f, 角速度: %.2f",
                    distance, angle_diff * 180 / M_PI, message.linear.x, message.angular.z);
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TurtleChaseNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}